#ifndef __EULER_ANGLE_CLASS_IMPL_DEFINE__
#define __EULER_ANGLE_CLASS_IMPL_DEFINE__
#include "EulerAngle.h"

EulerAngle::EulerAngle()
{
    this->heading = 0.0f;
    this->pitch = 0.0f;
    this->bank = 0.0f;
}

EulerAngle::EulerAngle(double heading, double pitch, double bank):heading(heading), pitch(pitch), bank(bank)
{

}
VOID EulerAngle::FromMatrixObjectToWorld(MatrixInformation& matrixInfo)
{
    double data[MATRIX_ROW_COUNT][MATRIX_COL_COUNT] = {0};
    matrixInfo.GetMatrixData(data);
    DEFINE_MATRIX_DATA_VAR(data);
    DOUBLE sp = -m32;
    // 检测万向节锁
    if (fabs(sp)>0.99999999999f)
    {
        pitch = HALF_PI * sp;
        bank = 0.0f;
        heading = atan2(-m23, m11);
    }
    else
    {
        pitch =  asin(sp);
        // atan2返回以弧度表示的 y/x 的反正切
        bank = atan2(m12, m22);
        heading = atan2(m31, m33);
    }
}
VOID EulerAngle::FromMatrixWorldToObject(MatrixInformation& matrixInfo)
{
    // 先获取矩阵的转置
    MatrixInformation Transpose = MatrixTranspose(matrixInfo);

    FromMatrixObjectToWorld(Transpose);
}
VOID EulerAngle::AngleLimit()
{
    pitch = AngleCorrection(pitch);
    if (pitch<-HALF_PI)
    {
        pitch = -PI - pitch;
        heading += PI;
        bank += PI;
    }
    else if (pitch> HALF_PI)
    {
        pitch = PI - pitch;
        heading += PI;
        bank += PI;
    }


    if (fabs(pitch) > HALF_PI-1e-4)
    {
        heading += bank;
        bank =0.0f;
    }
    else
    {
        bank = AngleCorrection(bank);
    }
    heading = AngleCorrection(heading);

}
double EulerAngle::AngleCorrection(double angle)
{
    angle += PI;
    angle -= floor(HALF_PI) * DOUBLE_PI;
    angle -= PI;
    return angle;
}
#endif // !__EULER_ANGLE_CLASS_IMPL_DEFINE__

